#include "action_interface/action/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_srvs/srv/set_bool.hpp"

using namespace std::chrono_literals;

class PublisherNode : public rclcpp::Node {
public:
  using MoveRobot = action_interface::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;

  PublisherNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "node: %s", this->get_name());
    pub_ = this->create_publisher<std_msgs::msg::String>("old_topic_name", 10);
    srv_ = this->create_service<std_srvs::srv::SetBool>(
        "old_service_name",
        std::bind(&PublisherNode::callback, this, std::placeholders::_1,
                  std::placeholders::_2));
    action_server_ = rclcpp_action::create_server<MoveRobot>(
        this, "old_action_name",
        std::bind(&PublisherNode::_handle_goal, this, std::placeholders::_1,
                  std::placeholders::_2),
        std::bind(&PublisherNode::_handle_cancel, this, std::placeholders::_1),
        std::bind(&PublisherNode::_handle_accepted, this,
                  std::placeholders::_1));
  }

  // service callback
  void callback(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
                std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
    (void)request;
    RCLCPP_INFO(this->get_logger(), "service callback called");
    response->success = true;
    response->message = "service callback called";
  }

  // action callback
  rclcpp_action::GoalResponse
  _handle_goal(const rclcpp_action::GoalUUID &uuid,
               std::shared_ptr<const MoveRobot::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Recieved goal request with distance %f",
                goal->distance);
    (void)uuid;
    (void)goal;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse
  _handle_cancel(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Recieved request to cancel goal...");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  void _execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Executing goal...");
    (void)goal_handle;
  }
  void
  _handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    using namespace std::placeholders;
    (void)goal_handle;
    // std::thread{std::bind(&Robot01::_execute_move,this,_1),goal_handle}.detach();
  }

private:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr srv_;
  // 声明一个action
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;
};

int main(int argc, char **argv) {
  // 初始化ros2的C++接口
  rclcpp::init(argc, argv);
  // 建立节点对象
  auto node = std::make_shared<PublisherNode>("remap_node");
  // 运行节点，检测退出信号
  rclcpp::spin(node);
  // 销毁节点并关闭C++接口
  rclcpp::shutdown();

  return 0;
}